/**
 * @file FSM_State_Jump.h
 * @author
 * @brief
 * @version 0.1
 * @date 2022-03-24
 *
 * @copyright Copyright (c) 2022
 *
 */

#ifndef FSM_STATE_JUMP_H
#define FSM_STATE_JUMP_H

#include "FSM_State.h"

/**
 *
 */
template <typename T>
class FSM_State_Jump : public FSM_State<T> {
public:
    FSM_State_Jump(ControlFSMData<T>* _controlFSMData);

    // Behavior to be carried out when entering a state
    void onEnter();

    // Run the normal behavior for the state
    void run();

    // Checks for any transition triggers
    FSM_StateName checkTransition();

    // Manages state specific transitions
    TransitionData<T> transition();

    // Behavior to be carried out when exiting a state
    void onExit();

    TransitionData<T> testTransition();

private:
    // Keep track of the control iterations
    int iter = 0;

    // JPos,存放电机角度
    Vec3<T> initial_jpos;    // 存放站立状态下三个电机的角度，四个脚均一致
    Vec3<T> start_jpos;      //存放起跳前，蹲下状态的电机角度
    //points，存放足端坐标点
    Vec3<T> start_points;    //存放蹲下状态的足端坐标
    Vec3<T> fin_points;      //存放跳跃结束前一瞬间的足端坐标

    void _SetJPosInterPts(const int& curr_count, int start_count, int finish_count, const Vec3<T>& ini,
                          const Vec3<T>& fin);

    bool _b_running = true;
    bool _b_first_visit = true;
    /* 0~10000:初始化    
    10000~15000：缩腿  
    15000~15010：跳跃   
    15010~20000：站立
    */
    int _count = 0;            //记录程序跑了几次
    int _waiting_count = 10000;    //,当_count小于_waiting_count时，执行_Initialization()
    int _star_count = 15000;      //_count到达_star_count时，开始跳跃
    int _finish_count = 15010;    //_count到达_finish_count时，跳跃完成

    bool _Initialization();
    void ComputeCommand();
    void _SafeCommand();
};

#endif    // FSM_STATE_JUMP_H